1 /*
2 * File: roll_ap.c
3 *
4 * Code generated for Simulink model 'roll_ap'.
5 *
6 * Model version : 1.164
7 * Simulink Coder version : 8.5 (R2013b) 08-Aug-2013
8 * C/C++ source code generated on : Mon Feb 03 08:13:50 2014
9 *
10 * Target selection: ert.tlc
11 * Embedded hardware selection: 32-bit Embedded Processor
12 * Code generation objectives: Unspecified
13 * Validation result: Not run
14 */
15
16 #include "roll_ap.h"
17 #include "roll_ap_private.h"
18
19 /* Output and update for referenced model: 'roll_ap' */
20 void roll_ap(const real_T *rtu_Phi, const real_T *rtu_Psi, const real_T *rtu_p,
21 const real_T *rtu_TAS, const boolean_T *rtu_APEng, const boolean_T *
22 rtu_HDGMode, const real_T *rtu_HDGRef, const real_T *rtu_TurnKnob,
23 real_T *rty_AilCmd, rtB_roll_ap *localB, rtDW_roll_ap *localDW,
24 real_T rtp_dispGain, real_T rtp_intGain, real_T rtp_rateGain,
25 real_T rtp_hdgGain)
26 {
27 /* local block i/o variables */
28 real_T rtb_HeadingMode;
29 real_T rtb_BasicRollMode;
30 real_T rtb_TKswitch;
31 real_T rtb_refswitch;
32
33 /* ModelReference: '<Root>/Heading Mode'
34 *
35 * Block requirements for '<Root>/Heading Mode':
36 * 1. 1.10. Heading Performance
37 */
38 Heading_Mode(rtu_HDGRef, rtu_Psi, rtu_TAS, &rtb_HeadingMode,
39 &(localDW->HeadingMode_DWORK1.rtb), rtp_hdgGain);
40
41 /* Outputs for Atomic SubSystem: '<Root>/Roll Reference'
42 *
43 * Block requirements for '<Root>/Roll Reference':
44 * 1. 1.3. Roll Hold Reference
45 */
46 /* Switch: '<S2>/ref switch' incorporates:
47 * Constant: '<S2>/minus six'
48 * Constant: '<S2>/six'
49 * Constant: '<S2>/zero'
50 * Logic: '<S2>/or'
51 * RelationalOperator: '<S2>/Ref Threshold'
52 * RelationalOperator: '<S2>/Ref Threshold1'
53 *
54 * Block requirements for '<S2>/ref switch':
55 * 1. 1.3. Roll Hold Reference
56 *
57 * Block requirements for '<S2>/minus six':
58 * 1. 1.3. Roll Hold Reference
59 *
60 * Block requirements for '<S2>/six':
61 * 1. 1.3. Roll Hold Reference
62 *
63 * Block requirements for '<S2>/zero':
64 * 1. 1.3. Roll Hold Reference
65 *
66 * Block requirements for '<S2>/or':
67 * 1. 1.3. Roll Hold Reference
68 *
69 * Block requirements for '<S2>/Ref Threshold':
70 * 1. 1.3. Roll Hold Reference
71 *
72 * Block requirements for '<S2>/Ref Threshold1':
73 * 1. 1.3. Roll Hold Reference
74 */
75 if (((*rtu_Phi) >= 6.0) || ((*rtu_Phi) <= (-6.0))) {
76 rtb_refswitch = *rtu_Phi;
77 } else {
78 rtb_refswitch = 0.0;
79 }
80
81 /* End of Switch: '<S2>/ref switch' */
82
83 /* Switch: '<S3>/Enable' incorporates:
84 * Logic: '<S2>/Not engaged'
85 * UnitDelay: '<S3>/FixPt Unit Delay1'
86 *
87 * Block requirements for '<S2>/Not engaged':
88 * 1. 1.3. Roll Hold Reference
89 */
90 if (!(*rtu_APEng)) {
91 } else {
92 rtb_refswitch = localDW->FixPtUnitDelay1_DSTATE;
93 }
94
95 /* End of Switch: '<S3>/Enable' */
96
97 /* Switch: '<S2>/TK switch' incorporates:
98 * Abs: '<S2>/Abs'
99 * Constant: '<S2>/three'
100 * RelationalOperator: '<S2>/TK Threshold'
101 * UnitDelay: '<S3>/FixPt Unit Delay1'
102 *
103 * Block requirements for '<S2>/TK switch':
104 * 1. 1.3. Roll Hold Reference
105 *
106 * Block requirements for '<S2>/Abs':
107 * 1. 1.3. Roll Hold Reference
108 *
109 * Block requirements for '<S2>/three':
110 * 1. 1.3. Roll Hold Reference
111 *
112 * Block requirements for '<S2>/TK Threshold':
113 * 1. 1.3. Roll Hold Reference
114 */
115 if (fabs(*rtu_TurnKnob) < 3.0) {
116 rtb_TKswitch = localDW->FixPtUnitDelay1_DSTATE;
117 } else {
118 rtb_TKswitch = *rtu_TurnKnob;
119 }
120
121 /* End of Switch: '<S2>/TK switch' */
122
123 /* Update for UnitDelay: '<S3>/FixPt Unit Delay1' */
124 localDW->FixPtUnitDelay1_DSTATE = rtb_refswitch;
125
126 /* End of Outputs for SubSystem: '<Root>/Roll Reference' */
127
128 /* Switch: '<Root>/Mode switch'
129 *
130 * Block requirements for '<Root>/Mode switch':
131 * 1. 1.2. Roll Hold Mode
132 * 2. 1.4. Roll Performance
133 * 3. 1.8. Heading Hold Mode
134 * 4. 1.10. Heading Performance
135 */
136 if (*rtu_HDGMode) {
137 localB->phiCmd = rtb_HeadingMode;
138 } else {
139 localB->phiCmd = rtb_TKswitch;
140 }
141
142 /* End of Switch: '<Root>/Mode switch' */
143
144 /* ModelReference: '<Root>/Basic Roll Mode'
145 *
146 * Block requirements for '<Root>/Basic Roll Mode':
147 * 1. 1.4. Roll Performance
148 * 2. 1.5. Roll Rate Limit
149 * 3. 1.6. Roll Angle Limit
150 * 4. 1.7. Aileron Angle Limit
151 */
152 attitude_controller(&localB->phiCmd, rtu_Phi, rtu_p, rtu_APEng,
153 &rtb_BasicRollMode, &(localDW->BasicRollMode_DWORK1.rtdw),
154 rtp_dispGain, 30.0, rtp_rateGain, 6.0, rtp_intGain, 5.0,
155 15.0);
156
157 /* Switch: '<Root>/Eng switch' incorporates:
158 * Constant: '<Root>/zero'
159 *
160 * Block requirements for '<Root>/Eng switch':
161 * 1. 1.1. Roll Autopilot Engage Control
162 *
163 * Block requirements for '<Root>/zero':
164 * 1. 1.1. Roll Autopilot Engage Control
165 */
166 if (*rtu_APEng) {
167 *rty_AilCmd = rtb_BasicRollMode;
168 } else {
169 *rty_AilCmd = 0.0;
170 }
171
172 /* End of Switch: '<Root>/Eng switch' */
173 }
174
175 /* Model initialize function */
176 void roll_ap_initialize(rtB_roll_ap *localB, rtDW_roll_ap *localDW)
177 {
178 /* Registration code */
179
180 /* block I/O */
181 (void) memset(((void *) localB), 0,
182 sizeof(rtB_roll_ap));
183
184 /* states (dwork) */
185 (void) memset((void *)localDW, 0,
186 sizeof(rtDW_roll_ap));
187
188 /* Model Initialize fcn for ModelReference Block: '<Root>/Basic Roll Mode' */
189 attitude_controller_initialize(&(localDW->BasicRollMode_DWORK1.rtdw));
190
191 /* Model Initialize fcn for ModelReference Block: '<Root>/Heading Mode' */
192 Heading_Mode_initialize(&(localDW->HeadingMode_DWORK1.rtb));
193 }
194
195 /*
196 * File trailer for generated code.
197 *
198 * [EOF]
199 */
200
|